#include "DSP2833x_Device.h"

#ifndef _DMC_H
#define _DMC_H


/*---------------------------------------------------------------------RDC---------------------------------------------------------------------*/
#define RDC_SCALER						0x1000U	//Rdc Scaler 12bit:0x1000
#define RSL_OFFSET_POS					2100UL//832UL//300Jc:2760UL		//Raw angular offset between resolver and motor positive direction
#define RSL_OFFSET_NEG					2000UL//672UL//300Jc:2620UL		//Raw angular offset between resolver and motor negative direction
#define RSL_POLE_PAIRS					2U			//Number of resolver pole pairs


/*---------------------------------------------------------------------Motor---------------------------------------------------------------------*/
#define BASE_RPM  						3000.F 		// BaseRpm (rpm)
#define T_PWM							0.00005F
#define F_PWM							20000U
#define MOTOR_POLE_PAIRS				2U			//Number of motor pole pairs

/*---------------------------------------------------------------------SPEED---------------------------------------------------------------------*/
#define ACC					  			450.F 			// Acceleration (rpm/s)
#define KP 								0.2F				// Parameter: proportional loop gain
#define KI  							9.5F*T_PWM				// Parameter: integral gain
#define KF  							0.2F				// Parameter: front gain
#define ULIM1							0.9F
#define DLIM1							-0.9F
#define ULIM3							1.F
#define DLIM3							-1.F


/*---------------------------------------------------------------------POSITION---------------------------------------------------------------------*/
#define V_MIN 							0.1F
#define DELTA_P1 						0.02F
#define DELTA_P2 						0.02F
#define DELTA_P3 						0.10F
#define K_P 							2.F


/*---------------------------------------------------------------------Dmc-test mode---------------------------------------------------------------------*/
#define MODE_CLOSE_LOOP						0U
#define MODE_OPEN_LOOP						1U
#define MODE_VF_CTL							2U
#define MODE_VF_TEST						3U
#define MODE_GET_OFFSET						4U
#define MODE_ZERO_SCAN						5U
#define MODE_THETA_CTL						6U

#define VF_U0								0.2F


/*---------------------------------------------------------------------DMC struct---------------------------------------------------------------------*/
struct DMC_DATA
{
	Uint16 uiEn;

	//Pos control
	//float32 fPosRef;				//Input		:	Postion reference (per unit)
	//float32 pPosFb;				//Input		:	Postion feedback (per unit)

	//Spd control
	float32 fSpdRef;				//Input(O)	:	Motor speed reference (per unit)
	float32 fSpdRefRamp;			//Output	:	Ramp nRef value (per unit)
	float32 fSpdFb;					//Output	:	Feed speed (per unit)

	//rdc and theta
	Uint16 uiRdcRaw;				//Input		:	Raw Data from RDC

	//pwm modulation
	float32 fUamp;					//Output	:	Motor stator voltage output (per unit)
	float32 fUthe;					//Output	:	Motor stator Electrical Angle (per unit)

	float32 fTa;					//Output	:	PWM-A
	float32 fTb;					//Output	:	PWM-B
	float32 fTc;					//Output	:	PWM-C

	//current
	float32 fCurrBus;
};
#define DEFAULT_DMC	{0U, 0.F, 0.F, 0.F, 0U, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}



/*---------------------------------------------------------------------FILTER struct---------------------------------------------------------------------*/
#define FILTER_DEPTH	64U
#define FILTER_CNT		128U
#define DEFAULT_QUENE	{																					\
							0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,	\
							0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,	\
							0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,	\
							0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F,	\
						}
struct FILTER
{
	float32 fDataIn;
	float32 fDataOut;
	Uint16 uiIndex;
	Uint16 uiCnt;
	float32 fQueue[FILTER_DEPTH];
};
#define DEFAULT_FILTER	{0.F, 0.F, 0U, 0U, DEFAULT_QUENE}


/*---------------------------------------------------------------------LOW_FILTER struct---------------------------------------------------------------------*/
struct LOW_FILTER
{
	float input;
	float K2;
	float K3;
	float output_last;
	float output;
};
#define LOW_FILTER_DEFAULTS {0,0,0,0,0}
/*---------------------------------------------------------------------function prototype---------------------------------------------------------------------*/
//0-Dmc test mode
Uint16 DmcZeroScan(struct DMC_DATA *v);
Uint16 DmcGetOffset(struct DMC_DATA *v);
void DmcTestMode(struct DMC_DATA *v , Uint16 uiMode);

//1-PostionCL
float32 PostionCL(float32 fPosRef , float32 fPosFb);
int16 HysteresisCtrl(float32 fValue);

//2-SpeedCL
void DmcSpeedCL(struct DMC_DATA *v);
float32 SpeedFeed(Uint16 uiRdcRaw);
float32 SpeedRamp( Uint16 uiEn ,float32 fSpdRef ,float32 fSpdRefRamp);
float32 SpeedPI( Uint16 uiEn , float32 fSpdRef , float32 fSpdFb);

//3-pwm
float32 GetThetaE(Uint16 uiRdcRaw, float32 fUamp);
void SPWMZ_Gen(float32 fUamp, float32 fTheta, float32 *fTa, float32 *fTb, float32 *fTc);
void SVPWM_Gen(float32 fUalpha, float32 fUbeta, float32 *fTa, float32 *fTb, float32 *fTc);

//4-DMC_common
float32 LimitF(float32 fVal, const float32 fMax, const float32 fMin);
void IClark(float32 fAlpha , float32 fBeta , float32 *fAs , float32 *fBs , float32 *fCs);
void Clark(float32 fAs, float32 fBs, float32 fCs, float32 *fAlpha, float32 *fBeta);
void Ipark(float32 fDs , float32 fQs , float32 fTheta , float32 *fAlpha , float32 *fBeta);
void Ipark(float32 fAlpha, float32 fBeta, float32 fTheta, float32 *fDs, float32 *fQs);
void Filter(struct FILTER *v);
void LowFilter(struct LOW_FILTER *v);

#endif
